#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "lifecycle_msgs/msg/state.hpp"        // 包含生命周期消息中的状态消息
#include "lifecycle_msgs/msg/transition.hpp"   // 包含生命周期消息中的转换消息
#include "lifecycle_msgs/srv/change_state.hpp" // 包含生命周期服务中的更改状态服务
#include "lifecycle_msgs/srv/get_state.hpp"    // 包含生命周期服务中的获取状态服务

static constexpr char const *lifecycle_node = "lifecycle_client";

class LifecycleClient : public rclcpp::Node
{
public:
    explicit LifecycleClient(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
        : Node("lifecycle_client", options)
    {
        client_ = this->create_client<std_srvs::srv::Trigger>("lifecycle_service");
        client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>("lifecycle_service_server/change_state"); // 创建更改状态服务客户端
    }

    void configure_node();

    void activate_service();

private:
    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_;
    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_; // 更改状态服务客户端
    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_;       // 获取状态服务客户端
};

void LifecycleClient::configure_node()
{
    // 创建并设置生命周期状态转换器
    auto actionRequest = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    // 配置状态改变
    actionRequest->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;

    // 等待节点的响应
    if (client_change_state_->wait_for_service(std::chrono::seconds(3)))
    {
        // 如果服务可用，会通过异步方式发送请求
        auto future_result = client_change_state_->async_send_request(actionRequest).future.share();
        // 使用 spin_until_future_complete 等待服务响应
        if (rclcpp::spin_until_future_complete(this->shared_from_this(), future_result) == rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_INFO(this->get_logger(), "rclcpp::FutureReturnCode::SUCCESS"); // 服务成功
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to call service."); // 服务失败
        }
    }
    else
    {
        RCLCPP_ERROR(this->get_logger(), "Service not available.");
    }
}

void LifecycleClient::activate_service()
{

    // Activate service
    auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
    if (client_->wait_for_service(std::chrono::seconds(1)))
    {
        auto future = client_->async_send_request(request);
        if (rclcpp::spin_until_future_complete(this->shared_from_this(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) // 修改这里
        {
            RCLCPP_INFO(this->get_logger(), "Service activated: %s", future.get()->message.c_str());
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to call service.");
        }
    }
    else
    {
        RCLCPP_ERROR(this->get_logger(), "Service not available.");
    }
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto client = std::make_shared<LifecycleClient>();

    // 调用生命周期转换服务来配置生命周期节点
    client->configure_node();

    // First, configure the server
    client->activate_service(); // 调用服务端以激活服务

    rclcpp::shutdown();
    return 0;
}
